
// Copyright (c) 2012, 2013, 2014 Pierre MOULON.

// This Source Code Form is subject to the terms of the Mozilla Public
// License, v. 2.0. If a copy of the MPL was not distributed with this
// file, You can obtain one at http://mozilla.org/MPL/2.0/.

#include <cstdlib>

#include "openMVG/sfm/pipelines/global/sfm_global_engine_relative_motions.hpp"
#include "openMVG/system/timer.hpp"
#include "openMVG/cameras/Cameras_Common_command_line_helper.hpp"
#include "openMVG/cameras/Camera_Pinhole.hpp"
#include "openMVG/cameras/Camera_Pinhole_Radial.hpp"
#include "openMVG/sfm/sfm_data_io.hpp"

using namespace openMVG;
using namespace openMVG::sfm;

#include "third_party/cmdLine/cmdLine.h"
#include "third_party/stlplus3/filesystemSimplified/file_system.hpp"

std::string sDataRootDir = "D:/experiment/Dubrovnik6K/";
std::string sBundlerOutFile = "bundle/bundle.orig.out";
std::string sImgNameList = "list.orig.size.txt";
std::string sOutDir = "D:/experiment/Dubrovnik/";

// construct sfm_data.json
int main( int argc, char **argv )
{
  using namespace std;
  // Load input SfM_Data scene
  SfM_Data sfm_data;

  sfm_data.s_root_path = sDataRootDir;

  std::vector<std::string> img_names;
  img_names.reserve( 8000 );
  std::vector<int> img_height_width;
  img_height_width.reserve( 16000 );

  std::ifstream ifs( sDataRootDir + sImgNameList, std::ios::in );
  if ( !ifs.is_open() ) {
    std::cout << "open file fail: " << sDataRootDir + sImgNameList << std::endl;
    return EXIT_FAILURE;
  }

  std::string img_name_tmp;
  int h, w;
  while ( ifs >> img_name_tmp ) {
    ifs >> h >> w;
    img_height_width.push_back( h );
    img_height_width.push_back( w );
    img_names.push_back( "image/" + img_name_tmp );
  }
  std::cout << "load image name: " << img_names.size() << std::endl;
  std::cout << "load image size: " << img_height_width.size() << std::endl;
  ifs.close();

  // read bundler data
  int num_camera, num_point;
  std::ifstream instream( sDataRootDir + sBundlerOutFile, std::ios::in );
  if ( !instream.is_open() ) {
    std::cout << "open bundler fail: " << sDataRootDir + sBundlerOutFile << std::endl;
    return EXIT_FAILURE;
  }

  std::string line_in_file;
  getline( instream, line_in_file );//header
  instream >> num_camera >> num_point;

  if ( img_names.size() != num_camera ) {
    std::cout << "num_camera error: " << num_camera << std::endl;
    return EXIT_FAILURE;
  }

  // read camera parameter
  for ( int i = 0; i < num_camera; ++i ) {
    double h = img_height_width[2 * i];
    double w = img_height_width[2 * i + 1];
    std::shared_ptr<View> ptr_view = std::make_shared<View>();
    ptr_view->id_intrinsic = i;
    ptr_view->id_pose = i;
    ptr_view->id_view = i;
    ptr_view->s_Img_path = img_names[i];
    ptr_view->ui_height = h;
    ptr_view->ui_width = w;
    sfm_data.views[i] = ptr_view;

    geometry::Pose3 pose;

    double f, k1, k2;
    instream >> f >> k1 >> k2;

    instream >> pose.rotation_( 0, 0 )
      >> pose.rotation_( 0, 1 )
      >> pose.rotation_( 0, 2 )
      >> pose.rotation_( 1, 0 )
      >> pose.rotation_( 1, 1 )
      >> pose.rotation_( 1, 2 )
      >> pose.rotation_( 2, 0 )
      >> pose.rotation_( 2, 1 )
      >> pose.rotation_( 2, 2 );

    Vec3 translation;
    instream >> translation[0] >> translation[1] >> translation[2];
    pose.center_ = -( pose.rotation().transpose() * translation );

    sfm_data.poses[i] = pose;

    using namespace openMVG::cameras;
    std::shared_ptr<cameras::IntrinsicBase> ptr_intrinsic = std::make_shared<Pinhole_Intrinsic_Radial_K3>
      ( w, h, f, w / 2.0, h / 2.0, 0.0, 0.0, 0.0 );  // setup no distortion as initial guess

    sfm_data.intrinsics[i] = ptr_intrinsic;
  }
  instream.close();

  std::cout << "views size: " << sfm_data.views.size() << std::endl;
  std::cout << "intrinsics size: " << sfm_data.intrinsics.size() << std::endl;
  std::cout << "extrinsics size: " << sfm_data.poses.size() << std::endl;


  // save to json format
  Save( sfm_data, stlplus::create_filespec( sOutDir, "sfm_data_converted", ".json" ), ESfM_Data( ALL ) );
  //return 0;


  return EXIT_SUCCESS;
}
